#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>

#include "c_pso.h"

#define L 1
#define C 1
#define R 1

#define END_T 8
#define DELTA_T .001
#define STEPS (END_T/DELTA_T)

// Max Control effort (0 is no limit)
#define MAX_CONT 10

// These control the weights for both settling time and overshoot in terms
// of fitness
#define OVER_WEIGHT .5
#define SET_WEIGHT .5
#define PROP_WEIGHT .01


// This function determines the fitness of the individual
void calc_fitness(pso_part *particle)
{
	pso_type k_p = particle->location[0];
	pso_type k_d = particle->location[1];
	pso_type k_i = particle->location[2];
	pso_type err = 1;
	pso_type err_old;
	pso_type err_sum = 0;
	pso_type err_sum_old;

	pso_type v_in = 0;
	pso_type v_c = 0;
	pso_type i_l = 0;
	pso_type i_tot = 0;
	pso_type v_c_old;
	pso_type i_l_old;
	int i;
	double set_time = 0;
	double over_shoot = 0;

	for (i = 0; i < STEPS; i++) {
		// Move last step values into old locations
		err_old = err;
		err_sum_old = err_sum;
		v_c_old = v_c;
		i_l_old = i_l;

		// Simulate circuit in discrete time quantities
		err = 1 - i_l_old;
		err_sum = err_sum_old + err * DELTA_T;

		v_in = k_p * err + k_d * (err - err_old) / DELTA_T + k_i * err_sum;

		if (MAX_CONT && (v_in > MAX_CONT)) v_in = MAX_CONT;
		if (MAX_CONT && (-v_in > MAX_CONT)) v_in = -MAX_CONT;

		i_tot = (v_in - v_c_old) / R;
		v_c = DELTA_T * (i_tot - i_l_old) / C + v_c_old;

		i_l = DELTA_T * (v_c / L) + i_l_old;

		// Check 2% settling time
		if (i_l > 1.02) set_time = i * DELTA_T;
		else if (i_l < .98) set_time = i * DELTA_T;

		// Check max overshoot
		if ((i_l - 1) > over_shoot) over_shoot = i_l - 1;
	}

	// Save fitness based on defined weights
	particle->fitness = OVER_WEIGHT * over_shoot + SET_WEIGHT * set_time;
	if (k_p > 0) {
		particle->fitness += PROP_WEIGHT * k_p;
	} else {
		particle->fitness -= PROP_WEIGHT * k_p;
	}

	return;
}
/*
// This function determines the fitness of the individual
void calc_fitness(pso_part *particle)
{
	pso_type sum = 0;
	pso_type center[DIM] = {42.5, 23.8, 21.2};
	int i;

	for (i = 0; i < DIM; i++) {
		if ((particle->location[i] - center[i]) > 0) {
			sum += (particle->location[i] - center[i]);
		} else {
			sum -= (particle->location[i] - center[i]);
		}
	}

	particle->fitness = sum;

	return;
}*/

// This function calls calc_fitness on the entire swarm
void calc_fitness_swarm(pso_swarm *swarm)
{
	int i;
	for (i = 0; i < N; i++) {
		calc_fitness(swarm->part_swarm + i);
		check_best(swarm->part_swarm + i, &swarm->best);
	}

	return;
}

// This function moves an individual by a given amount
void move_particle(pso_part *particle, pso_type move[DIM])
{
	int i;

	for (i = 0; i < DIM; i++) {
		particle->inertia[i] = (MOVE_RAT * move[i]) + ((1 - MOVE_RAT) * particle->inertia[i]);
		particle->location[i] += particle->inertia[i];
	}

	return;
}

// This function caculates required move for a particle and move it
void calc_move_particle(pso_part *particle, pso_part *best)
{
	pso_type move_array[DIM];
	int i;

	for (i = 0; i < DIM; i++) {
		move_array[i] = IND_WEIGHT * frand() * (particle->best_location[i] - particle->location[i]);
		move_array[i] += BEST_WEIGHT * frand() * (best->best_location[i] - particle->location[i]);
	}

	move_particle(particle, move_array);

	return;
}

// This function calls calc_move_particle on entire swarm
void move_swarm(pso_swarm *swarm)
{
	int i;
	for (i = 0; i < DIM; i++) {
		calc_move_particle(swarm->part_swarm + i, swarm->best);
	}

	return;
}

// This function checks whether an update is required for the given particle
// and updates if needed
void check_best(pso_part *new_part, pso_part **best_part)
{
	int i;
	if (new_part->fitness < new_part->best_fitness) {
		new_part->best_fitness = new_part->fitness;

		for (i = 0; i < DIM; i++) {
			new_part->best_location[i] = new_part->location[i];
		}
	}

	if (new_part->best_fitness < (*best_part)->best_fitness) {
		*best_part = new_part;
	}

	return;
}

// This function will perform a swap between to swarms, only called when needed
void swap_part(pso_swarm *swarm1, pso_swarm *swarm2)
{
	pso_part temp;
	int index1, index2;

	index1 = (int)(frand() * N);
	index2 = (int)(frand() * N);

	memcpy(&temp, swarm1->part_swarm + index1, sizeof(pso_part));
	memcpy(swarm1->part_swarm + index1, swarm2->part_swarm + index2, sizeof(pso_part));
	memcpy(swarm2->part_swarm + index2, &temp, sizeof(pso_part));

	return;
}

// This function initializes a particle with random startup values between 
// -MAX_VAL and MAX_VAL
void init_part(pso_part *particle)
{
	int i;
	for (i = 0; i < DIM; i++) {
		particle->location[i] = (frand() * 2 * MAX_VAL) - MAX_VAL;
		particle->best_location[i] = particle->location[i];
		particle->inertia[i] = 0;
	}
	calc_fitness(particle);
	particle->best_fitness = particle->fitness;

	return;
}

// This function will initialize and entire swarm
void init_swarm(pso_swarm *swarm)
{
	int i;
	for (i = 0; i < N; i++) {
		init_part(swarm->part_swarm + i);
	}

	swarm->best = swarm->part_swarm;

	return;
}

